#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/io/pcd_io.h"
#include "mqtt/async_client.h"

// MQTT配置
const std::string MQTT_SERVER_ADDRESS = "tcp://10.255.0.60:1883";
const std::string MQTT_CLIENT_ID = "ros2_pointcloud_receiver";
const std::string MQTT_TOPIC = "pointcloud/data";

class MqttPointCloudReceiver : public rclcpp::Node, public mqtt::callback
{
public:
    MqttPointCloudReceiver() : Node("mqtt_pointcloud_receiver")
    {
        // 创建点云发布者
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("output_pointcloud", 10);

        // 初始化MQTT客户端
        mqtt_client_ = std::make_unique<mqtt::async_client>(MQTT_SERVER_ADDRESS, MQTT_CLIENT_ID);
        mqtt_client_->set_callback(*this);

        // 设置连接选项
        mqtt::connect_options connOpts;
        connOpts.set_keep_alive_interval(20);
        connOpts.set_clean_session(true);

        // 连接MQTT服务器
        try {
            RCLCPP_INFO(this->get_logger(), "Connecting to MQTT server: %s", MQTT_SERVER_ADDRESS.c_str());
            auto connToken = mqtt_client_->connect(connOpts);
            connToken->wait();
            RCLCPP_INFO(this->get_logger(), "Connected to MQTT server");

            // 订阅主题
            mqtt_client_->subscribe(MQTT_TOPIC, 0)->wait();
            RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", MQTT_TOPIC.c_str());
        }
        catch (const mqtt::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "MQTT connection failed: %s", e.what());
            rclcpp::shutdown();
        }
    }

    ~MqttPointCloudReceiver()
    {
        // 断开MQTT连接
        if (mqtt_client_->is_connected()) {
            mqtt_client_->disconnect()->wait();
            RCLCPP_INFO(this->get_logger(), "Disconnected from MQTT server");
        }
    }

    // MQTT消息接收回调
    void message_arrived(mqtt::const_message_ptr msg) override
    {
        try {
            // 将接收到的数据反序列化为PCL点云
            std::stringstream ss(msg->get_payload_str());
            pcl::PointCloud<pcl::PointXYZ> cloud;
            pcl::PCDReader reader;
            reader.readBinaryCompressed<pcl::PointXYZ>(ss, cloud);

            RCLCPP_INFO(this->get_logger(), "Received pointcloud with %zu points", cloud.size());

            // 转换为ROS消息并发布
            sensor_msgs::msg::PointCloud2 output_msg;
            pcl::toROSMsg(cloud, output_msg);
            output_msg.header.frame_id = "map";  // 根据实际情况设置坐标系
            output_msg.header.stamp = this->get_clock()->now();
            publisher_->publish(output_msg);
        }
        catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "Error processing MQTT message: %s", e.what());
        }
    }

    // MQTT连接丢失回调
    void connection_lost(const std::string& cause) override
    {
        RCLCPP_WARN(this->get_logger(), "MQTT connection lost: %s", cause.c_str());
        // 尝试重新连接
        if (!cause.empty()) RCLCPP_WARN(this->get_logger(), "Cause: %s", cause.c_str());
        
        RCLCPP_INFO(this->get_logger(), "Attempting reconnect");
        mqtt::connect_options connOpts;
        connOpts.set_keep_alive_interval(20);
        connOpts.set_clean_session(true);
        
        mqtt_client_->reconnect()->wait();
        RCLCPP_INFO(this->get_logger(), "Reconnected to MQTT server");
        mqtt_client_->subscribe(MQTT_TOPIC, 0)->wait();
    }

    // MQTT交付完成回调
    void delivery_complete(mqtt::delivery_token_ptr token) override {}

private:
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    std::unique_ptr<mqtt::async_client> mqtt_client_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MqttPointCloudReceiver>());
    rclcpp::shutdown();
    return 0;
}
